#!/usr/bin/env python

# Copyright 2012 Colin McDonough, Troy Velius

import roslib; roslib.load_manifest('pr2_hri')
import rospy
from std_msgs.msg import String
import actionlib
import pr2_hri.msg

class Demo2Client(object):

  def __init__(self, name):
    self.client = actionlib.SimpleActionClient('HriActionServer', pr2_hri.msg.HriAction)
    self.client.wait_for_server()

    # Raise Camera
    goal_raise = pr2_hri.msg.HriGoal
    goal_raise.goalString = String('raise_camera_to_eye')
    self.client.send_goal(goal_raise)

    self.client.wait_for_result()

    result_raise = self.client.get_result()

    rospy.loginfo(result_raise.resultString.data)

    # Move In
    goal_move_in = pr2_hri.msg.HriGoal
    goal_move_in.goalString = String('move_in')
    self.client.send_goal(goal_move_in)

    self.client.wait_for_result()

    result_move_in = self.client.get_result()

    rospy.loginfo(result_move_in.resultString.data)

    # Move Left
    goal_lower = pr2_hri.msg.HriGoal
    goal_lower.goalString = String('move_left')
    self.client.send_goal(goal_lower)

    self.client.wait_for_result()

    result_lower = self.client.get_result()

    rospy.loginfo(result_lower.resultString.data)

    # Move Right
    goal_lower = pr2_hri.msg.HriGoal
    goal_lower.goalString = String('move_right')
    self.client.send_goal(goal_lower)

    self.client.wait_for_result()

    result_lower = self.client.get_result()

    rospy.loginfo(result_lower.resultString.data)

    # Lower Camera
    goal_lower = pr2_hri.msg.HriGoal
    goal_lower.goalString = String('lower_camera_from_eye')
    self.client.send_goal(goal_lower)

    self.client.wait_for_result()

    result_lower = self.client.get_result()

    rospy.loginfo(result_lower.resultString.data)

    # Right Arm Down
    goal_lower = pr2_hri.msg.HriGoal
    goal_lower.goalString = String('right_arm_down')
    self.client.send_goal(goal_lower)

    self.client.wait_for_result()

    result_lower = self.client.get_result()

    rospy.loginfo(result_lower.resultString.data)

if __name__ == '__main__':
  rospy.init_node('Demo2Client')
  hriDebug = rospy.Publisher('hriDebug', String)
  hriDebug.publish(String(rospy.get_name()))
  Demo2Client(rospy.get_name())
  rospy.spin()
